#include "drv_gimbal.h"
#include "bsp_can.h"
#include "bsp_dbus.h"
#include "math.h"
#include "apl_pid.h"

PID gimbal_yaw, gimbal_pitch;
PID gimbal_yaw_speed, gimbal_pitch_speed;

void PID_gimbal_init() {
	PID_init_angle(&gimbal_yaw, CAN_6020_MAXrpm, &k_angle_gimbal);
	PID_init_angle(&gimbal_pitch, CAN_6020_MAXrpm, &k_angle_gimbal);
	PID_init_speed(&gimbal_yaw_speed, CAN_6020_MAXV, &k_speed_gimbal);
	PID_init_speed(&gimbal_pitch_speed, CAN_6020_MAXV, &k_speed_gimbal);
}

float angle_yaw, angle_pitch;
short  motor_angle_input[2] = {0};

void Gimbal_init(void) {
	PID_gimbal_init();
	angle_yaw = motor_data[5].ecd;
	angle_pitch = motor_data[4].ecd;
	for(char i = 0; i < 2; ++i) motor_angle_input[i] = 0;
	CAN_cmd_gimbal(motor_angle_input);
	HAL_Delay(500);
}

void Gimbal_RC(void) {
	angle_yaw += (rc_ctl.rc.ch[3] - 1024.0) * 50 / 1320;
	if(angle_yaw > 8191) angle_yaw -= 8191;
	if(angle_yaw < 0) angle_yaw += 8191;
}	


void Gimbal_set_angle(float yaw_set, float pitch_set) {
	angle_yaw = yaw_set, angle_pitch = pitch_set;

	float target_sp_yaw = PID_angle_output(&gimbal_yaw, angle_yaw, motor_data[5].ecd);
	
	motor_angle_input[1] = (short)(PID_speed_output(&gimbal_yaw_speed, target_sp_yaw, motor_data[5].speed_rpm));
	
	CAN_cmd_gimbal(motor_angle_input);
}